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Ma chi opera la verita viene alia luce, perche appaia chiaramente che le sue 

opere sono state fatte in Dio. Gv 3,21 

Non basta guardare, occorre guardare con occhi che vogliono vedere, che 

credono in quello che vedono. Galileo Galilei 
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Chapter 1 

Introduction 


There was a time when the robots were thought in the Asimov’s mind as 
intelligent mechanical structures that obey to the following three laws; 

1. A robot may not injure a human being or, through inaction, allow a 
human being to come to harm; 

2. A robot must obey orders given it by human beings except where such 
orders would conflict with the First Law; 

3. A robot must protect its own existence as long as such protection does 
not conflict with the First or Second Law. 

In Asimov’s science fiction vision there was already the presence of two 
important aspects of the robotics: to give orders and safety. 

According to the trends of the robotics research in a few of decades, 
robots will be able to share the same dynamically changing environments 
with the human beings and these latter will not be worried to share the 
same place with some mechanical friends. To arrive to this scenario the 
research has to move its point of view. Industrial robots, well-known as 
manipulators, have been developed thinking them as operators in a cell, 
where they do some planned tasks, moving and rotating the end-effector 
following a predetermined path. 
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Industrial robots work in a separate cell where no human beings can 
be, according to safety standards. Advanced robotics moves away from this 
view. The robot has to share the same human environment. To afford 
this idea, the research has to re-think the way how a robot interacts with 
the environment and a human. A robot has to become an autonomous 
system; it has to accomplish some tasks interacting independently of the 
surroundings . Furthermore, a robot needs to cooperate with human beings 
without damaging their lives; the research needs a new approach to the 
interaction between humans and robots, finding a way to develop machines 
that act like humans (in behaviours and in physical structures) and use low 
forces, similar to the human ones. 

In this way, a robot can be used not only on an assembly line but in many 
places, also the ones where humans cannot work, for example in a critical 
situation of a nuclear central where it is dangerous to expose people to the 
radioactivity, while for the robot could be less dangerous, this is just one 
example; we could think also to space robots or underwater robots. There 
are already some robots used in the cited above contexts. 

Service robotics is also a very useful application of robotics; the robots 
are seen as helpers in the human activities, for example in the domestic 
environments for cleaning operations, or to help erderly people. 

In advanced robotics, one of the most important issues to address is to 
equip the robots with the capabilities to react to human actions or even take 
the initiative to interact with them, by taking decision on the basis of data 
coming from the sensors and background knowledge. 

The most important areas of interest of the advanced robotics research 
are; actuation and power systems, haptic and interaction technologies, ar- 
tihcial intelligence and learning, sensors fusion, motion planning. A new 
way to design actuation and power systems has to be found especially to 
improve energy savings and to get movements similar to those of humans; 



why a robot cannot move like a human? Haptic and interaction research 
tries to develop the sense of touch of the haptic interfaces and to improve 
the way how the robot reacts to the sense of touch. 

As described before, in advanced robotics the sensor system is the base 
to tackle some problems. According to the kind of the sensors that you have 
got on your robot, you can perform different tasks. The information given 
by the sensors are not always of the same type, so they need to be filtered 
in a way to extrapolate the right information: this is the objective of the 
sensor fusion. 

Within motion planning, an important task to obtain is to avoid an 
obstacle situated nearby the path that the robot has to follow. How can we 
achieve this objective? A human being would move away to protect all the 
parts of his body, moving it without hitting other obstacles. Then, when the 
human being passes the obstacle, he goes back to the action he was doing 
before the obstacle came. Until today the research tried to pursue the same 
behaviours on mobile robots. 

A mobile robot, equipped with proximity sensors, would act similarly but 
not perfectly like a human being does. The main limitations currently are: 
until today a robot does not find a way to prevent itself from the obstacle in 
a global way and furthermore it deviates from the obstacle without a global 
perception of the surroundings. 

As it will be showed in this work {2nd Chapter) the robotics research 
has elaborated some techniques to achieve the obstacle avoidance but all of 
them do not think of a robot in its completeness, they compute commands 
to move away from the obstacle as it was a single piece: few sensors calculate 
the distances between the robot and the obstacles and according to these 
measures an algorithm elaborates the commands. 

Few sensors can permit to obtain only a robot conservative representa¬ 
tions in the environment, they allow to approximate the position of a robot 
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as a geometric primitive in the space. Even more so, for a manipulator what 
the research has done is to think of it in a conservative way, due to the larger 
complexity related to the high number of degrees of freedom describing its 
configuration. 

This work tries to elaborate a new approach to the obstacle avoidance; 
why a robot has to accomplish in a conservative way the obstacle avoidance? 
Is it possible to think to obstacle avoidance based on distributed proximity 
sensing? 

In the 3rd Chapter we try to describe the reasons to the need of a new ap¬ 
proach to obstacle avoidance: a behavioural approach based on distributed 
proximity sensing. The 4th Chapter describes how we implemented the new 
approach, we used two toolboxes of Simulink; SimMechanics and the VR 
toolbox. 

Afterwards in the 5th Chapter conclusions are drawn describing the re¬ 
sults and some possible future developments. 


10 



Chapter 2 

State of the Art 


Real-time obstacle avoidance is a prerequisite for Human-Robot Interac¬ 
tion, for service robotics and for all the robotic applications working into a 
dynamically changing environment specifically for autonomous robotic sys¬ 
tems. Generically, the obstacle avoidance is a task to achieve together with 
other tasks, we could think to a manipulation task: while the robot is doing 
its job we would like to allow the avoidance of an incoming unpredicted ob¬ 
stacle. Furthermore obstacle avoidance actions can include the self-collision 
avoidance concept namely to permit to a manipulator, a mobile robot or a 
humanoid robot to avoid the collision between its links. Past research has 
already found ways to get the obstacle avoidance. In dynamically changing 
environments what really matters is a way to get advantage of the kinematic 
redundancy and the ability to merge single elementary behaviours to com¬ 
pose a complex one: Null-Space-Based behavioural control does this using 
a projection mechanism. We must give the same importance to the algo¬ 
rithms to obtain the obstacle avoidance: they have been divided into motion 
re-planning algorithms and reactive control algorithms. 

Below is reported a brief review of the state of the art of planning algo¬ 
rithms, reactive control algorithms and behavioural-based control. 
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2.1 Planning Algorithms 


The first kinds of re-planning algorithms used were the Road maps, where 
the planning algorithms use a retraction method based on the generalized 


Voronoi Diagrams [I] (Figure 2.1). Consequently a cell decomposition al¬ 
gorithm, which finds a path on a connectivity graph, was adopted [2]. Re¬ 
planning algorithms were interpreted also in a probabilistic way, trying to 
explore the free configuration space, near the robot, using a space sampling. 

There are two types of algorithms: the Probabilistic Road Map (PRM) 
a multiple-query stochastic method [3] , and the Rapidly-exploring Random 
Ti’ee {RRT) |lj (Figure [2(^ a single-query stochastic method, where the free 
configuration space is explored only in the interesting parts. 

All the previous mentioned methods are planning algorithms very useful 
in static environments, they are executed in the planning layer of the robot. 
These are performed obviously thanks to the information received by the 
sensors. 



Figure 2.1: A generalized Voronoi Diagram. 
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Figure 2.2: An RRT is biased by large Voronoi regions to rapidly explore, 
before uniformly covering the space. 

2.2 Reactive Control Algorithms: The Artificial 
Potential 


A different approach is to avoid the obstacles directly using a control algo¬ 
rithm. We found in the literature different methods. They are based on a 
reactive control resulting from the computation of an artificial potential (an 


example in Figure 2.3) . A very important improvement was done by the 
introduction of the repulsive forces, due to an artificial potential, acting as 
forces in the operational space of a robot. Citing Khatib in his work [5j : 


The manipulator moves in a field of forces. The position to he 
reached is an attractive pole for the end effector and obstacles 
are repulsive surfaces for the manipulator parts. 


The control of manipulators in operational space is based on the choice 
of the forces F as control inputs, r are the corresponding torques (or forces) 
applied to the joint actuators. The relationship between forces and joint 
torques (or forces) is the following: 
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Figure 2.3: An example of a potential field with local and global minimum. 


T = J^{q)F (2.1) 

The key idea of the method is to compute the repulsive forces due to the 
artificial potential, which consists generically of two terms: 

Ua{x) = Ud{x) + Uo{x) (2.2) 

Ud{x) is the artificial potential that attracts the robot to the desired con¬ 
figuration. Uo{x) is the non-negative continuous and differentiable function 
that creates at each point a potential to reject the manipulator when it is 
too near to the obstacle. The forces applied are: 

Fa{x) = Fd{x) + Fo{x), (2.3) 

where 

Fd{x) = -V[Ud{x%Fo{x) = -V[Uo{x)]. (2.4) 
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To obtain asymptotic stabilization of the system he added dissipative forces 
proportional to x. 

Fm{x) =-kp{x - Xd) - Kx (2.5) 

Khatib in his work used the following potentials: 


Uo{p) 


Ud{x) = ]^kp{x - 


Xdf 

2 

, Po 


( 2 . 6 ) 


(2.7) 


0, otherwise 

where po represents the limit distance of the potential field and p is the 
shortest distance to the obstacle. In this approach it is clear that from all of 
the points subjected to the potential, it is selected the one with the shortest 
distance to the obstacle. The control is achieved using the following force (it 
is equal to zero when p > po ) considering a single obstacle: 


where 


Fo{x) = T] 


1 

P 


1 \ 1 (9/9 
Po ) p^ 3x 


( 2 . 8 ) 


dp dp dp dp 

dx dx dy dz 

In his approach Khatib outlined that the obstacle avoidance problem 
could be divided in two stages: 

• at high level control, generating a global strategy for the manipulator’s 
path in terms of intermediate goals; 

• at the low level, producing the appropriate commands to attain each 
of these goals, taking into account the detailed geometry and motion 
of manipulator and obstacle, and making use of real-time obstacle 
sensing. 


(2.9) 
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The complexity of tasks that can be implemented with this approach 
is limited. In a cluttered environment, local minima can occur in the re¬ 
sultant potential field. This approach has a local perspective of the robot 
environment, the robot may follow in a stable position before reaching its 
goal. 

According to Khatib’s vision any point of the robot can be subjected 
to the artificial potential field. The superposition property (additivity) of 
potential fields enables the control of a given point of the manipulator with 
respect to this obstacle by using the sum of the relevant gradients. Per each 
obstacle Oi described by a set of primitives Pp we have: 

^Oi,psp = ^ ^{Pp,psp) ( 2 - 10 ) 

p 

Alternatively we can have a reactive control algorithm in a kinematic 
way, just mapping the gradient of the total potential in the joint velocity of 
the manipulator using the transposed Jacobian. 

Let q be the configuration of the robot, Pi{q) (for i = 1,..P — 1) is a 
control point situated on the manipulator where the potential are calculated. 
The control point for the end effector is Pp. The velocities applied to the 
manipulator are the following; 

P-i 

q = -Y^jf {q)VUr{pi) - Siq)VUt{Pp) (2.11) 

While Khatib proposed the repulsive potential (the one in equation 
other types of potential were developed. 

2.2.1 The Harmonic Potential 

The main idea behind the Harmonic Potential method m) is to have an 
artificial potential, without local minima, using the Laplace equation. The 
solutions of the Laplace equation are called harmonic functions. 


(2.7)) 
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In the real world, many physical problems are described by the Laplace 
equation. An example is the incompressible fluid, a steady state electric 
charge distribution and also a steady state temperature distribution also 
follows the Laplace equation. The equations regulating the physical be¬ 
haviours of an incompressible fluid are: 

= 0,xeR^ (2.12) 

From which we obtain the following system: 

X = —V(j){x),x{0) = XoeR^ (2-13) 

The first important property of a harmonic function is the principle 
of superposition, which follows from the linearity of the Laplace equation. 
That is,i;/)i and 4>2 are harmonic, then any linear combination of them is also 
harmonic and a solution of the Laplace equation. 

A drawback of HPFs is that they cannot be directly used for generating 
virtual driving forces as in Khatib work is done. 

In this case a potential gradient is replaced by velocity field. 

2.2.2 Circular Fields 

Another important approach to artificial potentials is the Circular Fields 
algorithm. The physical phenomena behind the Circular Fields (Figure [2.4[ ) 
is the generation of N artificial electromagnetic-fields by the currents cir¬ 
culating in N wires, of indefinite length, situated on the robot surfaces [7j. 
The forces applied to the robot are the following: 


N 

Fj = x X Bi 

i=l 

(2.14) 

Cj X Iifii 

Bi = h ,2 di 

(2.15) 


'i 
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where 1^ is the virtual current factor, Cj is the current direction vector 
of surface element i, di is the surface element area, li = ||x —Xj|| is the 
distance of the current position of the point mass x, and Xi is the position of 
the obstacle surface element i of obstacle j. Ultimately, a few modifications 


goal 



Figure 2.4: Principle of the current definition for Circular Fields for obstacle 
j with surfaces i and robot n. 

have been proposed for this approach [8]. 

The algorithms presented above are all used as part of the robot control 
algorithm. The next presented algorithm merges planning with the forces 
obtained by an artificial potential. 

2.3 Elastic Strips 

The Elastic Strip [9] is a framework used to merge reactive motion to the 
classical motion planning. The framework defines an elastic tunnel that 
contains the candidate path generated by a planner. Virtual Robots move 
on homopatic path computed from the candidate path using a potential 
field-based control algorithm. The virtual robot moves in the elastic tunnel 
(example in Figure [2^ as it was guided by strings: one to repulse the robot 
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from an obstacle situated under a distance threshold, and others to permit 
the robot to go back to the candidate path if the obstacle would recede. 
Differently from other types of motion planning algorithm, in this framework 
there is not the need to explore the entire configuration space, instead it 
maps proximity information from the environment into the configuration 
space, using the kinematics of the manipulator. 



Figure 2.5: Elastic tunnel in the presence of an obstacle. 












-AVA 




^1 


q. 


Figure 2.6: Principal structure of elastic strip. 


To define the elastic strip we have to add incrementally to the candidate 
path the path generated by the forces calculated from an internal and an 
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external potential. The external potential used is the following; 


Ve{p) 


' 1 

-kr{do - d{p))‘^, if d{p) < do 
< ^ 


(2.16) 


0, otherwise 

V 

where d{p) is the distance from p to the closest obstacle, do defines the region 
of influence around obstacles, and kr is the repulsion gain. The force that 
acts on a single point p (the closest one) is the following: 


^ ^ (2.17) 

where d is the vector between p and the closest point on an obstacle. The 
internal force, corresponding to the inertial potential is: 


M-l 


iTf = kc{ 




1 






(2.18) 


where d* is the distance 


P) - P 


,i+l 


in the initial path and kc is the con¬ 


traction gain of the elastic strip (its structure is depicted in Fig. 2.6). The 
robot moves through the new path contained in the elastic strips. The forces 
are never applied directly to the robot but they are mapped to joint dis¬ 
placements using the kinematic model of the manipulator. This effectively 
replaces configuration space exploration with a directed search, guided by 
work space forces. 

In case of redundant manipulators, a third potential (Vp) is introduced. 
The potential Vp is associated to the posture, it can be used to define a 
preferred posture for the robot in absence of other constraints. 

The integration of planning and control allows to delay the exploration 
of the configuration space necessary for plan generation until execution, 
executing re-planning more efficiently. The elastic strips is applied to a 
single point (for a manipulator, for each link there is a single point, chosen 
as the one with the shortest distance to the obstacle). This approach is 
a good strategy to solve the problem of the local minima because it is a 
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local and global method in the same time, but elastic strips cannot replace 
planning itself: if changes in the environment are substantial and the elastic 
strip framework cannot find a valid candidate path, the re-invocation of a 
complete planner becomes necessary. 

2.4 The skeleton algorithm 

The skeleton algorithm in |in) is an important step forward in reactive col¬ 
lision avoidance because it outlines a way to compute the position of the 
control points and the corresponding Jacobian matrices needed for control. 
The algorithm, developed for the self-collision avoidance, is composed of 
these four steps: 

• building a proper model of the robot, namely the skeleton, useful for 
analytical computation; 

• finding the closest point to a possible collision along the skeleton, 
namely the collision points; 

• generating repulsion forces; 

• computing avoidance torque commands to be summed to the nominal 
torques for the controller. 

Once built, the model of the robot, which in general can be built by 
hand or it could be derived {the skeleton) automatically from a proper kine¬ 
matic description (e.g via a Denavit-Hartenberg table), we have to find the 
collision points. They move along the segments of the skeleton. Hence, 
the direct kinematics computation can be carried out in a parametric way 
for a generic point on each segment by simply replacing the link length in 
the homogeneous transformation, relating two subsequent frames with the 
distance of the collision point from the origin of the previous frame. 
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The collision points Pa,c and are found computing the minimum 
distance between the two segments. To the computation of the avoidance 
torques, it is necessary to compute the Jacobians associated with the colli¬ 
sion points, i.e. the matrices Ja,c and Jb^c describing the differential mapping 
of pa,c and with the joint velocities q of the whole structure, i.e. in com¬ 

pact form: 


Pa,c 


da,c 

Pb,c 


-1 

o 

_1 


We have to notice that the positions of pa,c and pb^c vary along the 
segments as the manipulator is moving. 

This algorithm calculates the forces as repulsion forces of an artificial 
potential 


Uc{p) 


2 k{dsf;art dmin do) ; if drfYiiji <C do dstart 

0, otherwise 


(2.19) 


where dmin is the minimum distance between two collision points, dstart is 
the starting distance where the force has to act: points farther than dstart 
are not subject to any repulsion. Moreover, do is the limit distance around 
the skeleton where a collision may occur: in the case of cylindrical links, do 
is the radius of the section of the link. The following forces are defined if, 

dmin ^ do T dstart • 


k[dstart dmin + do) , \ ■ in nn\ 

fa,c = ^ - {Pa,c - Pb,c) - DaPa,c (2.20) 

(^min 

kydstart ~ dmin + do) , \ m ■ /'ll N 

Jb,c = -j - {Pb,c - Pa,c) - DbPb,c (2.21) 

dmin 

where Da and Db are positive definite matrices. Multiple collision points 
on the same segment may need to be considered, whenever more segments 
are close to a possible collision. The damping matrix is used to increase the 
values of forces applied to the collision points for the presences of multiple 
collision points. 
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The last step of the algorithm is to compute the avoidance torques cor¬ 
responding to the repulsion forces via the Jacobian transpose defined above 
i.e ; 

T = \ jT' jT 
^ [ '^a,c'^b,c 

These torques are added to the ones used for the control. As an alter¬ 
native to the presented technique, the skeleton algorithm could be applied 
also for a velocity control based implementation. 

2.5 The Null-Space-based behavioural control 

Behaviour-based approaches are methodologies to design the control archi¬ 
tecture of artihcial intelligence systems. The key idea of behaviour-robotics 
is that intelligence of the robotic system is provided by a set of behaviours, 
designed to achieve specific goals, that are activated on the basis of sen¬ 
sor information. A behaviour is expressed through a function of the robot 
configuration that measures the degree of fulfilment of the task. 

There are different approaches to handle multiple elementary tasks to 
be executed simultaneously. The solutions to this problem of behavioural 
coordination can be divided in two categories: competitive methods and 
cooperative methods. 

In the competitive methods the coordination can be viewed as a compe¬ 
tition among behaviours; only one behaviour wins and only its response is 
sent to the robot for execution. An example of competitive methods is the 
layered architecture proposed in m- Layers have different priority levels 
and a hierarchy solves the conflict among the tasks, higher-level tasks can 
subsume the lower-level ones; it is also known as subsumption architecture. 
Cooperative methods merges the output of more than one behaviour at a 
time. A supervisor gives as output a solution, calculated as the sum of all 
motion commands (one for each task) opportunely multiplied by a gain vec- 


fa, c 

fb, c 
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tor, which can dynamically change on the basis of sensor information: the 
motor schema control ([E]) is an example of cooperative behaviour coor¬ 
dination. Differently from the competitive approach in the cooperative one 
the ontput is a linear combination of all behaviours, all of them participate 
to the control of the robot. 

An important step forward is done by the Null-Space-based Behavioural 
(NSB) control f|13j). This technique is based on some properties of the in¬ 
verse kinematics (an overview is reported in [Hj). For a redundant manipu¬ 
lator the inverse kinematics problem admits an infinite number of solutions 
and a criterion to select one of them is needed. 

A lot of work has been done considering the problem of redundancy res¬ 
olution at the inverse differential kinematics level. The simplest method is 
based on the use of the pseudo-inverse of the Jacobian matrix: this guaran¬ 
tees optimal reconstruction of the desired end-effector velocity -in a least- 
squares sense- with the minimum-norm joint velocity. Redundancy can be 
exploited to meet constraints additional to the solution of the inverse kine¬ 
matics problem. A possible constraint is the obstacle avoidance. Generically, 
a term is added for local optimization of a scalar criterion, i.e a term pro¬ 
portional to the gradient of the criterion is projected onto the nnll space of 
the Jacobian matrix so that the end-effector task is not affected. 

The task-space augmentation is another approach to the resolution of the 
redundancy. It introduces a constraint task to be fulfilled along with the 
end-effector task, so an augmented Jacobian matrix is set-up whose inverse 
gives the sought joint velocity solution. 

A stone on which the NSB is based is the framework of the task-priority 
strategy m. [IS]). In this approach the conflicts between the end-effector 
task and the constraint task is resolved by assigning an order of priority to 
the given tasks and then satisfying the lower priority task only in the null 
space of the higher priority task. 
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In the NSB for each behaviour we need a task variable a G K^, and we 
define p G the system configuration, then we have: 

u = f{p) (2.22) 

with the corresponding differential relationship: 

d = = J(p)u (2.23) 

where m is the task function dimension, J G is the configuration 

dependent task Jacobian matrix, and v := p G is the system velocity. 

Notice that the only case of interest is m < n; otherwise if m > n, the task 

would be unfeasible or the null space of a full rank J{p) would be empty 

thus preventing the possibility of controlling any other task. 

Consider a generic behaviour k defined by the task variable ak having a 
desired value and a Jacobian Jk- The reference velocity of the generic 
task can be calculated by the Closed Loop Inverse Kinematics Algorithm 
(CLIK) as 

Vk — Jk{^d,k T -^fcdfc)) (2.24) 

where = j'^{JkJ^)~^ (when m < n and rank{Jk{p)) = m), Ak is a 
suitable constant positive-definite matrix of gains and ak is the task error 
defined as ak = ad,k — crk- The term Akdk is added to counteract the 
numerical drift due to discrete-time integration m describes how to choose 
Ak to ensure the stability of the algorithm). 

A velocity vector for each behaviour is computed as if it was acting alone; 
then, before adding the single contribution to the overall vehicle velocity, 
a lower-priority behaviour is projected onto the null space of the higher- 
priority behaviours so as to remove those velocity components that would 
conflict with it. An example of the merging of three tasks could be: 

Vd = Vi + {I - jIJi)[v2 + {I - 4'^2)t'3] (2.25) 
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Equation (2.25) has a geometrical interpretation. Each task velocity is 


computed as if it were acting alone; then, before adding its contribution to 
the overall robot velocity, a lower-priority task is projected onto the null 
space of the immediately higher-priority task so as to remove those velocity 
components that would conflict with it. 

A supervisor might be considered in order to dynamically change the 
relative task priorities. The research until today does not suggest the type 
of the supervisor to use and at the same time does not give any condition 
of stability for it. The lower-priority tasks are fulfilled only in a subspace 
where they do not conflict with the ones having high priority. This is clearly 
an advantage with respect to the competitive approaches, where one single 
task can be achieved at once, and to the cooperative approaches, where 
the use of a linear combination of each single task’s output has as result 
that no single task is exactly fulfilled and furthermore the designer needs to 
heuristically tune the parameters. 

In the case of multiple non-conflicting tasks the NSB does not guarantee 
that the lower priority tasks is instantaneously achieved with the sub-optimal 
velocity. Nevertheless, in the considered case, the closed loop ensures that 
the error of secondary tasks converges to zero. 

2.6 On distributed sensor systems 

A distributed sensor system is generally thought as an array of sensors. 
They are distributed on a surface where we want to collect some informa¬ 
tion. In order to obtain the obstacle avoidance we focus our attention on 
proximity sensors. We might think to a robot skin that needs to be covered 
with a sensor array to detect proximity of an object. The proximity sense 
is the sense that tell us how far from the skin of the robot is an object. A 
proximity sensor complements the gap between vision sensors and tactile 
sensors: vision sensors can see wide areas but sometimes occlusion occur; 
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tactile sensors are useful to detect a contact on a snrface. An impressive 
example of distributed proximity sensor system is the ’’Net-Structure Prox¬ 
imity Sensor” developed by the UEC Shimojo Laboratory in Tokio [18]. In 
this sensor, infrared LED and photo-transistors are used as the detection 
elements. The sensor outputs the center position of the nearby object and 
the distance from the sensor surface.The sensor can be laid out like a net 
to cover arbitrary surfaces. Response time is less than 1 ms without re¬ 
gard to the number of detection elements. The circuit diagram (see Eigure 


2.7) of Net-Structure Proximity Sensor are shown below. An infrared LED 


is allocated in a set with each of the photo-transistors. The infrared light 
emitted from the LED reflects on a nearby object, and then is radiated to the 
array of photo-transistors. Here, current distribution occnrs in the circuit 
according to the difference of the amount of the light received by the photo¬ 
transistors. By calculating the first moment of the current distribution from 
voltages on four terminals (Vsl, Vs2, Vs3, Vs4), the center position of the 
object is obtained. Furthermore, since the amount of the light received on 
the photo-transistors changes according to the distance between the sensor 
surface and the object, the distance (z) is also estimated from the total 
amount of the cnrrent in the circuit. 

Moreover the same group has developed still a new design of Net-Structure 
Proximity Sensor. The basic structure is the same as the Net-Structure 
Proximity Sensor, but photo-transistors are arranged on the edges of the m 


X n matrix as indicated in figure 2.8 


This sensor is implemented on a cylindrical snrface. Each photo-transistor 
faces outside in radial direction. In this geometric arrangement, the output 
of the center position (xc,yc) corresponds to the direction of a nearby object, 
and so the sensor can detect 360 degrees all around seamlessly. This sensor 
is called Ring-Shaped Proximity Sensor . 
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Figure 2.7: Net-Structure Proximity Sensor. 
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Figure 2.8: Ring-Shaped Proximity Sensor. 
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Chapter 3 

A new distributed approach 
to the obstacle avoidance 

3.1 Research Question 

As anticipated in the introduction, the objective of this work is to develop 
an algorithm for the obstacle avoidance of a mobile manipulator in a dy¬ 
namically changing environment. The aim is to let the robot autonomously 
move away from the obstacles situated on its path. What the robotics re¬ 
search has done until today is to achieve this task in a conservative way, 
considering the robot as a single body and finding a way to escape from the 
obstacle. As the Chapter^sYiowed, there are many methods to achieve this 
objective, but these methods act in a conservative way: 

• the artificial potential sees the robot subjected to a field of forces, com¬ 
puted in few so called control points situated along the manipulator, 
the force field’s gradients are computed and then projected into the 
transposed Jacobian; 

• the elastic strips method permits the robot to move in a strip, which 
represents the path to follow, this path is modified as the obstacles 
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move in the strips: also in this method we need a few of control points 
to perform the task. 

Both methods were developed using few discrete sensors: laser scanners, 
radars, cameras. These kind of sensors and the related algorithms are able 
to pursue the task in a conservative way: there is a lack of information 
obviously due to the sensors used and also because the algorithms are based 
on too few information, each body of the robot is described only from a few 
control points. 

In addition some requirements that an obstacle avoidance algorithm has 
to satisfy can be identihed. They can be established after a careful analysis 
of some cases. 

It is intuitive that the avoidance of obstacles needs some sensors which 
have a field of view as large as possible. 

A robot equipped with a single camera or a single laser scanner can have 
information of the surrounding things only if these are in the sensor’s field 
of view, what does it happen if something hits the robot from behind? The 
safety is guaranteed only for the parts of the robot close to sensor locations, 
while some parts of the robot are not because no proximity information is 
available. 

Furthermore, to pursue the avoidance in real-time and in daily contexts 
we do not need a structured environment, a robot has to perform the avoid¬ 
ance everywhere: from the inside of an house to the street, in an airport or 
in a train station, in a market or in a farm. It needs no help from outside, 
just its sensor system has to help it. The robot has only the knowledge 
received from the sensors mounted on it, it should know only the path to 
follow and it should not know the obstacle position in advance. 

The robot has to execute the avoidance as a task called only when it 
is needed, when there is not any obstacle, the robot has to follow its path. 
The change between the two tasks has to be autonomous: with its own 
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intelligence the robot changes the task to afford. 

The requirements to be fulfilled can be summarized as follows : 

• Safety: the robot performs the obstacle avoidance in a secure way, 
hitting no one; 

• Availability: the robot is always available to perform this task, in every 
possible environment condition; 

• Real-Time: the obstacle avoidance is done in a very short time, the 
time required to permit a prompt avoidance; 

• Distributed Sensing: the sensors are situated along all the surface of 
the robot, and we need only proximity information detected from the 
sensors mounted on the robot, anymore information situated in the 
environment is redundant; 

• Autonomous system: the robot has autonomously change the priority 
of the tasks to perform. 


3.2 The distributed perception approach 


The requirements of Safety and of Availability describe an attitude to the 
protection of the robot. Recalling the Asimov’s Laws, the robot needs to be 
protected not only for its own safety but also for safety of objects or human 
beings sharing its workspace. 

Our idea is to think a robot protected using N parallel springs distributed 
along all the surface of the robot, taking in account all the elastic energy 
associated to them. 

As said, the robot is considered as protected by N springs with a certain 


rest length. As an example, in Fig. 3.2 the safety volumes of all the springs 
are represented and activated, even though each spring is active only in prox¬ 
imity of an obstacle. In fact, when an obstacle touches one or more springs 
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(in the sense that it comes close enough to the corresponding sensors), the 
total elastic energy associated to these springs increases. To perform the 
obstacle avoidance, the robot has to move away from the obstacle so as to 
minimize the total energy, i.e. the sum of the elastic energies of all the 
springs touched by the obstacle. 

If we think of an anthropomorphic arm, to each body we should mount 
some proximity sensors . We imagine to associate an elastic energy to a 
sort of protective buffer centered in each sensor location. The elastic energy 


(figure 3.1) permits us to use distances and the direction of the minimum 
distance as the only information to compute the algorithm. It’s a distributed 
approach independent from the robot geometry. The springs make up a 



Figure 3.1: An example of avoidance using elastic energy 


protection area (figure 3.2) where the control mechanisms are activated to 


perform the avoidance. The protection area is a rubber sphere where is 
defined an elastic energy. On each sensor there is this protection area. On 
all its surface, the robot has these points of protection, we call them control 
points or sensor points. There is no more a single point of view, all the 
sensors distribute the perception along the robot. 

The obstacle avoidance is obtained keeping constant to the initial value 
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Figure 3.2: The springs protecting the robot 


all the elastic energy associated to the sensors. 

Since we use a proximity sensor skin (based on a finite number of N 
sensors distributed along the robot), the objective is : 


N 

= '^^k = Ki (3.1) 

k=l 

where is the elastic energy associated to the sensor point k of the 
robot surface and Ki an initial constant value. 


3.2.1 The proposed control algorithm 


We have used the NSB control to perform the avoidance in an autonomous 
way. 

Our method can be applied to a generic kinematic open chain. There 
is only one hypothesis: the sensors provide to the control unit the point at 
minimum distance from the surface of the obstacle. Looking at the figure 


(3.3), in order to formalize the approach, assume that the /cth sensor, located 
in the point , measures the distance dk between the nearest point on the 
obstacle Po^, and the sensor itself as well as the direction of the minimum 
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Figure 3.3: A generic arm with a sensor point and an obstacle. 

distance expressed by the unit vector Vd^. Assuming to adopt the Denavit- 
Hartenberg convention and that all vectors expressed in frame 0 (the first 
hxed DH-frame of the chain) have no superscript, with reference to Fig. |3.3[ 
let: 

• ^ — [qi 92 • • • 9n]^ ^ tie the vector of conhguration variables; 

• 64 = [O 0 0 i]'^; 

• pf-i = Ai{qi) ■ ■ ■ Alz‘l{qi-i)e 4 be the position vector (homogeneous 
coordinates) of the origin of the DH-frame fixed to the link z; 

• Zi-i = Riiqi) ■ ■ ■ RlZi{qi-i)zo be the z axis (along joint i) of the frame 
fixed to link z — 1; 

• Pof, be the position vector of the obstacle point at minimum distance 
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from the kth sensor; 


• PskiQ) — Piio) ~ ^%Q)dpl. be the position vector of the kth sensor 
point, being dpi, is the constant position of the kth sensor, assnmed 
mounted on link i, with respect to frame i fixed to link i. 

The NSB control requires to define some task functions to perform, the 
behaviours. As described before our behaviours are two: the obstacle avoid¬ 
ance as primary behaviour and the path following as the second one. 

Our desired behaviour is described by these equations, N is the number 
of sensors: 


o-d = 0; drf = 0; 


(3.2) 


(3.3) 


N 

k=l 

The sigma function is the sum of the pseudo-energy of each ^th sensor. 
q G is the configuration of the robot of n degrees of freedom. 


The pseudo-energy (figure (3.4)) is a continuous function defined as fol¬ 


low: 


^kiq) 


■^{dk - Tkf, if dk < rk 
< ^ 


(3.4) 


0, otherwise 

V 

It depends on the distance between the obstacle and the robot and the 
value rk G R, dehned as the resting length of the spring. We define f G R 
as the activation threshold of the supervisor, /s G i? as the radius of the 
sensor’s field of view and dk = \\Pok — Psk{Q)\\ fbe distance between the 
position vector of the generic sensor point and the position vector of the 
obstacle point situated at minimum distance, both expressed in the base 
frame. The desired energy is zero. It means that we want to keep 


dk = rk 


(3.5) 
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when the obstacle avoidance behaviour is in active. 


We must satisfy these constraints: 


/ < fs] 

(3.6) 

f < rk] 

(3.7) 

rk < fs 

(3.8) 


The equation (3.6) tell us that we can calculate the pseudo-energy only 
when the sensor can get information about the environment. The equation 


(3.7) permits us to avoid making unnecessary calculations. According to the 


equations (3.6),(3.7), (3.8) we have to choose Vk so that we have : 


f <rk< fs 


(3.9) 


When the obstacle avoidance task is activated, we have an initial error (equa- 

eo = e(/) (3.10) 


tion (3.10)) in the CLIK: 


In discrete time the value of cq can compromise the convergence of the CLIK 
algorithm (see the discussion in m)- 

Each task in the NSB control has to be computed in the CLIK algorithm, 
before to merge both behaviours using a task combination function. 

In literature there is no a specific way to design the supervisor in the NSB 
control. We have elaborated a regularized supervisor: a convex combination 
of the different tasks, the task combination function. 

The 7 gains play an important role in relations between the different 
behaviours, they may be seen as the elastic constants of springs that bring 
the robot on the tasks of different behaviours. 


3.2.2 The task combination function 

A velocity vector for each behaviour is computed as if it was acting alone; 
then, before adding the single contribution to the overall configuration ve¬ 
locity, a lower-priority behaviour is projected onto the null space of the 
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Figure 3.4: The pseudo-energy function. 

higher-priority behaviours so as to remove those velocity components that 
would conflict with it. A supervisor is used to change the priorities of the 
tasks. 

A simple way to do it, it is to use a finite state machine, changing from 
state to state in a discrete way. It means that the supervisor does not permit 
to merge the two tasks and gradually select one of them: there is only one 
behaviour acting at a time. This kind of supervisor can generate in the 
robot sudden accelerations. 

We think the supervisor not as a crispy selector of behaviours but as a 
continuous function that acts as a weight to the velocities of the different 
behaviours, i.e. as it is a convex combination of the different tasks. 

Depending on the number of the tasks and the kind of application to 
develop you can choose different functions. 
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Our application uses two behaviours; one for the avoidance one for the 


path following. They are defined as: 


Qo = Jl{q)[^d + 7odd] + (/ - jKq)Joiq))qg (3.11) 

qg = Jlixd + ^gixd - x)) (3.12) 

where Qo G is the configuration velocity vector due to the obstacle avoid¬ 
ance, Qg G is the configuration velocity vector due to the achievement of 
the goal position, (xd, Xd) G R^ is the desired position and velocity vector 
expressed in the base frame, Jg is a M x n matrix. We dehned the following 
continuous function: 


where 


X{d) = — aTctan{—K{d — /)) -|- - 

TT 2 


d = min(dfc) 
k 


(3.13) 


(3.14) 


where dk = \\Pok ~ and / is a threshold,we have to declare f < to 
obtain an initial energy different from zero, as we said before. It is the value 
where the function has its mid-scale value. 

Note that in this way a sort of “grey zone” is defined where a priority 
among the tasks is not “crisply” defined. Outside of this zone (namely for d 
large or small enough), the priority is established by the classical null-space 
projection method. Varying K varies the slope of the function. 


The hgure (3.5) shows an example of the function. 


We can use also a piece-wise linear function (figure (3.6)) defined as 
follows: 


X{d) 


- 2e —^ 

' 1, if d < (/ - e) 

0, if d> {f + e) 

\ 


(3.15) 
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Figure 3.5: An example of the task combination function. 

where 

d = mm{\\Po^ - PsJ) (3.16) 


1 I-1-^-1- ^- 1-^^-1 

0 . 9 - 

0 . 8 - \ 

0 . 7 - 

0 . 6 - \ 

S 0 . 5 - \ 

0 . 4 - 
0 . 3 - 
0 . 2 - 
0.1 - 

S.os, 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 

d[m] 


Figure 3.6: An example of the piece-wise linear task combination function. 
The system velocity is then defined as the convex combination: 

q = X{d)(^Jl{q)[&d + 'yodd\ + {I - Jl{q)Jo{q))q^ + {1 - X{d)){qg) (3.17) 

where: 

(fd = (^d- cr{q); 
qg = J^ixd + lg{xd - x)); 
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(3.18) 

(3.19) 











When the minimum distance between the obstacle and our robot is less 
then the threshold our supervisor gives more importance to the obstacle 
avoidance behaviour, in this case \{d) goes to 1 as much as d is less then /; 
otherwise X{d) goes to 0. 


3.2.3 The Jacobians Jo and Jg 

Jo is the one associated to the a function and it is the gradient of the 
elastic energy found before. It depends on the position of the sensor, the 
configuration of the robot and the minimum distance from the obstacle. We 
have calculated it in a closed form: 


^ ^ _ da{q) _ ^ dek{q) 

Defined dk = \\Po^. — and the unit vector Vk 


(3.20) 

II p”*" have: 


dekjq) 

dq 


^ T^Pskid) -rj / 

-{dk-rk)Vk —5 -, Jdk<rk 

< dq 

0, otherwise 

V ' 


(3.21) 


and is the Jacobian for the fcth sensor. 

dq 

Instead Jg is the geometric Jacobian associated to the configuration q of 
the robot. 


3.2.4 The architecture of the control unit 

The control algorithm described before can be developed in more ways, we 
suggest an architecture. The control has to respect the system velocity in 
[3T71 

The design consists of different modules: 

• the sensors system, where the proximity information are elaborated; 

• the a unit, that calculates the energy for each sensor; 
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• the two units that compute the jacobians for the CLIK implementa¬ 
tion; 

• the k{q) unit that calculates the direct kinematics function of the robot 

• the task combination function unit. 


The sensors system elaborates the information coming from the sensors 
and it sends them to the a and Jo units and to the supervisor one. The 


figure (3.7) shows a scheme of the system. 

The control unit needs the path (x^, x^) generated from a planning unit. 
In our idea the planning algorithm does its work separately from the control. 
Note that tg is the sampling time and the gains 7 ^ and 70 are strictly related 
to its value. 



Figure 3.7: The control scheme. 
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Chapter 4 

The Kuka Youbot Simulator 


To evaluate the new algorithm we developed a dynamical simulator. The 
robot simulated is the Kuka Youbot a mobile robot with 8 degrees of free¬ 
dom. The first three are for the base, two prismatic joints and a revolute 
one. The last fives are for the arm mounted on its base. It’s a small new 
robot useful for doing research. 

The simulator is developed using two toolboxes of Simulink (Matlab) ; 
SimMechanics and VR ToolBox. 

4.1 SimMechanics and the VR ToolBox 

SimMechanics is a toolbox that extends the Simulink capabilities. It allows 
the modelling and simulation of mechanical systems. It is based on the 
principle to be able to model a mechanical system without the knowledge of 
the dynamic equations associated to it. It uses blocks that represent bodies, 
joints, constraints and drivers. In order to model a mechanical system we 
must know only the structure of the various entities. 

An example would be the classic model of an anthropomorphic arm; 
it consists of a base on which are applied two revolute joints having axes 
perpendicular to each other, one of these is connected to the first arm of 
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the manipulator, which then is connected to a second arm through another 
joint. For the realization of the model of the arm we need three bodies (the 
base and the two arms) and three revolute joints. 

For the modelling of the bodies we used a block of the library called 
Body, in it these fields are inserted: 

• mass; 

• moment of inertia; 

• position and rotations of the CS (Coordinate System,a frame that can 
be connected to a point of the block); 

• position and rotation of the CG (Center of Gravity, the frame of the 
Center of Gravity of the body). 

We can found in the SimMechanics library the following blocks: 

• Ground, to model the earth; 

• Machine Environment, to define the properties of the environment 
in which you are working, for example, the dehnition of the gravity 
vector; 

• Shared Machine Environment,to share the same properties of the en¬ 
vironment among multiple objects that do not belong to the same 
kinematic chain. 

Therefore by simple connections between blocks SimMechanics allows the 
realization of a simulator. Eor each GS is possible to connect the sensors to 
read the information of: position, velocity, angular velocity, rotation matrix, 
acceleration, angular acceleration. In addition to the sensors in the library 
there are the actuators for both the joints and the bodies: the actuators 
permit us to control joints and bodies in position and in terms of forces. 
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The Virtual Reality Toolbox allows the use of a virtual reality viewer in 
Simulink. The 3D environments used in the VR Toolbox must be developed 
through language VRML97. Many CAD drawing softwares are enabled to 
export in this format. For this work we have generated the VRML model of 
the robot starting from the one made by Kuka for the open source software 
Blender. 

We must add that we took two conversions to obtain the final VRML 
model: one from the software Blender to 3D Studio Max, and another one 
from the latter to the VRML editing software. 

Each object has several properties, among which the most significant 

are; 

• Center, it is the vector that indicates the point at which the center of 
the object is fixed in the space; 

• Rotation, it is the vector and the angle indicating the rotation angle 
of the object in axis-angle representation; 

• Translation, it is the vector that indicates the translational motion of 
the object relative to its initial position; 

• Shape, it is a property divided into Appearance and Geometry in which 
is expressed the geometry and the colours of the object. 

These properties can be changed when the object is inserted in a simula¬ 
tor using Simulink block VRsink. In it you can associate a 3D scene to a 
simulator and you can modify the properties of the same 3D scene with the 
classic Simulink signals. 

4.1.1 The Kuka Youbot robot 

The Kuka Youbot is a robot formed by a omnidirectional base and a manip¬ 
ulator with hve degrees of freedom at the end of which is placed a gripper 
to grasp objects. 
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Its mechanical structure is represented by an open kinematic chain, with 


8 degrees of freedom: 3 for the base and 5 for the manipulator. 
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Figure 4.1: Description of the Youbot 

The omnidirectional base can be represented with two prismatic and one 
revolute joints, the manipulator with five revolute joints. 

The base is made of steel and it weighs 20 kg, instead the arm is made of 
an alloy of aluminium and magnesium. The arm supports 0.5 kg of payload. 
It presents a repeatability of 1 mm. We report a list of the kinematics 
and dynamics information of the Youbot. In the list below each rigid body 
has a relative position and orientation with respect to its parent frame. 
Orientation is described in Euler angles. The composite rotation convention 
is Yaw Pitch Roll. Joint axis is the axis of rotation for revolute joint or 
the axis of translation for prismatic joint. Joint axis is specified in the joint 
frame. The dynamics related parameters are mass, relative pose of center 
of mass and moment of inertia tensor. 

• Base Frame: Relative positionXYZ=”Omm 0mm 84mm”, Mass=19.803 
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Kg; 

• front-left wheel joint: Parent Joint = ’’base frame”, Relative posi- 
tionXYZ = ”228mm 158mm -34mm”, Joint type = ’’continuous”, Joint 
axis= ”0 1 0”, Mass = ” 1.40kg”; 

• front-right wheel joint: Parent joint=” base frame”,Relative positionXYZ 
= ”228mm -158mm -34mm”,Joint type = ’’continuous”,Joint axis = 
”0 1 0”, Mass = ” 1.40kg”; 

• back-left wheel joint: Parent joint = ’’base frame”,Relative positionXYZ 
= ”-228mm 158mm -34mm”, Joint type = ’’continuous”, Joint axis = 
”0 1 0”, Mass = ” 1.40kg”; 

• back-right wheel joint: Parent joint = ’’base frame”. Relative posi¬ 
tionXYZ = ”-228mm -158mm -34mm”, Joint type = ’’continuous”. 
Joint axis = ”010”, Mass = ” 1.40kg”; 

• plate: Parent joint = ’’base frame”. Relative positionXYZ = ”-159mm 
0mm 46mm”, Joint type = ’’fixed”. Mass = ”2.397kg”; 

• arm base frame: Parent joint = ’’base”. Relative positionXYZ = 

” 143mm 0mm 46mm”, Joint type = ’’fixed”, Mass = ”0.961kg”; 

• arm joint 1: Parent joint = ’’arm base frame”. Relative positionXYZ 
= ”24mm 0mm 115mm”, OrientationZYX = ”0° 0° 180°”, Joint type 
= ’’revolute”. Joint axis = ”0 0 1”, Joint limits = ”-169° 169°”, 
Mass = ” 1.390kg”, max. torque = ”9.5Nm”, principal axis of iner¬ 
tia; positionXYZ = ” 15.16mm 3.59mm 31.05mm”, orientationZYX 
= ”180° 20° 0°”, inertia = ”xx = 0.0029525 kg m2,yy=0.0060091 
kg m^,zz=0.0058821 kg m^”; 

• arm joint 2: Parent joint = ’’arm joint 1”, Relative positionXYZ = 
”33mm 0mm 0mm”, Orientation ZYX = ”-90° 0° 90°”, Joint type = 
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’’revolute”, Joint axis = ”0 0 1”, Joint limits = ”-65° 90°”, Mass = 

” 1.318kg”, max. torque = ”9.5Nm”, principal axis of inertia;positionXYZ 
= ” 113.97mm 15.0mm -19.03mm”, orientationZYX = ”-90° 0° -90°”, 
inertia = ”xx = 0.0031145% yy = 0.0005843% m? zz=0.0031631% . 

• arm joint 3: Parent joint = ’’arm joint 2”, Relative positionXYZ 
=” 155mm 0mm 0mm”, OrientationZYX = ”-90° 0° 0°”, Joint type = 
’’revolute”. Joint axis = ”0 0 1”, Joint limits = ”-151° 146°”, Mass 
= ”0.821kg”, max. torque = ”6.0Nm”, principal axis of inertia: po¬ 
sitionXYZ = ”0.13mm 104.41mm 20.22mm”, orientationZYX = ”0° 

0° 90°”,inertia = ”xx = 0.00172767% yy = 0.00041967% 
zz=0.0018468% m^”. 

• arm joint 4- Parent joint = ’’arm joint 3”, Relative positionXYZ 
= ”0mm 135mm 0mm”, OrientationZYX = ”0° 0° 0°”, Joint type 
= ’’revolute”, Joint axis = ”0 0 1”, Joint limits = ”-102.5° 102.5°”, 

Mass = ”0.769kg”, max. torque = ”2.0Nm”; principal axis of iner¬ 
tia; positionXYZ = ”0.15mm 53.53mm -24.64mm”, orientationZYX = 

”0° 180° 40°”,inertia = ”xx = 0.0006764% yy = 0.0010573% 
zz=0.0006610% m?”. 

• arm joint 5: Parent joint = ’’arm joint 4”, Relative positionXYZ = 
”0mm 113.6mm 0mm”; OrientationZYX = ”0° 0° -90°”, Joint type 
= ’’revolute”. Joint axis = ”0 0 -1”, Joint limits = ”-165° 165°”, 

Mass = ”0.687kg”, max. torque = ’’l.ONm”, principal axis of in¬ 
ertia; positionXYZ = ”0mm 1.2mm -16.48mm”, orientationZYX = 
”0°90°0°”; inertia = ”xx = 0.0001934% yy = 0.0001602% 
zz=0.0000689% 

• gripper base frame: Parent joint = ’’arm joint 5”, Relative position 
=”0mm 0mm 57.16mm”, OrientationZYX = ”180° 0° 0°”, Joint type 
= ’’fixed”. Mass = ”0.199kg”, principal axis of inertia: positionXYZ 
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= ”Omm 0mm 28.9mm”, orientationZYX = ”180° 0° 90°”, inertia = 
”xx = 0.0002324% w? yy = 0.0003629% m? zz=0.0002067% m?". 

• gripper left finger joint: Parent joint = ’’gripper base frame”, Relative 
position = ”0mm 8.2mm 0mm”, Joint type = ’’prismatic”. Joint axis 
= ”010”, Joint limits = ”0mm 12.5mm”, Mass = ’’0.010kg”. 

• gripper right finger joint: Parent joint = ’’gripper base frame”, Rela¬ 
tive position = ”0mm -8.2mm 0mm”, Joint type = ’’prismatic”. Joint 
axis = ”0 -1 0”, Joint limits = ”0mm 12.5mm”, Mass = ’’0.010kg”. 

For the computation of the Jacobian we have find out also the Denavit- 
Hartenberg parameters of the Youbot. They are: 
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4.2 The Simulator 

The realized simulator consists of two macro blocks: one models the mobile 
base and the other one implements the manipulator. 

The model of the base has been achieved with the connection of a body 
(which has been described with the moment of inertia and the geometry of 
the object) to a prismatic joint with two degrees of freedom {respeet to the 
axis X and y axis) and to a revolute joint to implement a rotation about the 
z axis. 
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The model of the arm consists of 5 blocks, one for each joint-body con¬ 
nection. For every body that makes up the kinematic chain was transcribed 
the moment of inertia, mass, and CS for each point where you can connect 
a sensor useful for finding the needed information. 
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Figure 4.2: The Simulator. 

In the simulator is also included the VR Sink for visualization of the 3D 
model in real time. It has been connected to the measurements made by 
the sensors for the movement of the joints and for the movements of the 
machine. 

The 3D environment realized currently includes the presence of: 

• Youbot Kuka, it is the robot model; 

• plane, it is a 3D model of a floor, on which the robot is put; 

• object, is a generic object situated in the scene. 
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Figure 4.3; 3D scene 


4.3 Description of rigid bodies, actuators and sen¬ 
sors 


Each rigid body within the simulator has the following components, pre¬ 


sented in Figure 4.4 


• Joint, it is the joint to which the body is connected,for the manipulator 
we used hve revolute joints, for the base two prismatic and one revolute 
joints; 

• IC, the initial conditions in which the joint is placed; 


Body, it is the body linked to the joint; 


Joint Sensor,{in Figure 4.5) it is the sensor connected to the joint, it 
permits to obtain different information (angle, angular velocity, angu¬ 
lar acceleration, torque for the revolute joints; position, velocity and 
acceleration for prismatic joints); 


Joint Actuator,(m. Figure 4.7 ) it is the actuator that allows the appli¬ 
cation of the signals in terms of position or force (for prismatic joints), 
or in angle of rotation and torque (for revolute joints); 
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The realized simulator provides a path planned in joint space. It is pos¬ 
sible to make a planning in terms of forces simply by changing the properties 
of the Joint Actuators. In position a Joint Actuator requires three signals 
indicating: position, velocity and acceleration. Obviously, these three quan¬ 
tities are angular when it comes to a revolute joint. In the case where it is 
desired to control the robot in terms of torque then it is possible to associate 
signals in forces. 

For each joint must be given the axis around which rotation occurs {Axis 
of Action^ whose coordinates can be related to Frame World, or to the frame 
of the body that precedes or follows the same joint) or if it is prismatic the 
vector on which the translation takes place. In the block Joint (in Fignre [T^ 
it also indicates the nnmber of sensors and actuators connected to it. 



Joint Actuatorl 


Figure 4.4: The model of a single body 

Each introduced body (in Figure [4^ must be connected to a joint and 
we have to specify: 

• the mass expressed in specified units; 

• the inertia] 

• the CG and the CS, the first defined as the point in which the center 
of mass of the body is situated, the latter points in which it is possible 
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Figure 4.5: Window for the sensor 

to apply other bodies, joints, sensors or actuators. For the realization 
of the simulator they have been used substantially to connect the prior 
joint to the next one, taking into account of course the exact position 
expressed in the manufacturer’s specifications. 

4.3.1 How SimMechanics analyses the motion 

The Parameters tab of the Machine Environment dialog allows you to choose 
the analysis mode you want to simulate in. You make this choice via the 
Analysis mode pull-down menu. In the case of linearization, use the Lin¬ 
earization tab to set the size of the small perturbations. See the Analyzing 
Motion chapter for detailed instructions and examples concerning the mo¬ 
tion analysis modes. 

By choosing one of these analysis modes, you implement the type of 
motion analysis you want. Here there is a list of the analysis modes offered: 
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Figure 4.6: Window for the joint 

• Forward dynamics, computes the positions and velocities of a system’s 
bodies at each time step, given the initial positions and velocities of 
its bodies and any forces applied to the system; 

• Linearization, computes the effect of small perturbations on system 
motion through the Simulinklinmod command; 

• Trimming, enables the Simulinktrim command to compute steady- 
state solutions of system motion; 

• Inverse dynamics (open-loop), computes the forces required to produce 
a specified velocity for each body of an open-loop system; 

• Inverse dynamics (closed-loop), computes the forces required to pro¬ 
duce a specified velocity for each body of a closed-loop machine. 

SimMechanics uses an ODE solver to solve the system’s equations of 
motion, typically in tandem with a constraint solver. Simulink provides an 
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9 Block Parameters: Joint Actuator4 




Joint Actuator 

Actuates a Joint primitive with generalized force/torque or 
linear/angular position, velocity, and acceleration motion signals. 
Base-follov/er sequence and joint axis determines sign of forward 
motion. Inputs are Simulink signals. Motion input signals must be 
bundled into one signal. Connect to Joint block to see Connected to 
primitive list. 


Actuation 

Connected to 
primitive: 

Actuate v.fith: 


|ri 



1 Motion 



Angular units: rad 



Angular velocity units: I rad/s 



Angular acceleration units: |rad/s^2 



[ OK ] I Cancel | | Help | [ Apply | 


Figure 4.7: Window for the actuator 


extensive set of ODE solvers that represent the most advanced numerical 
techniques available for solving differential equations in general and equa¬ 
tions of motion in particular. The Solver pane of a model’s Simulation 
Parameters dialog box allows you to select any of these solvers for use by 
Simulink in solving the model’s dynamics. 

The Dormand-Prince solver (ode45) that Simulink uses by default works 
well for many mechanical systems, but might require too much time to solve 
systems that are stiff, that is, have bodies that move at widely varying 
speeds or that have many discontinuities in their motion. An example of a 
stiff system is a pair of coupled oscillators in which one oscillator is much 
lighter than the other and hence oscillates much more rapidly. Any of the 
following solvers might require significantly less time than the default solver 
to solve a stiff system: 

• odelSs, variable-order solver based on a backward differentiation rule 
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Figure 4.8: Window to modify the body 


(variant of Gear’s method); 

• ode23t, trapezoidal rule solver. Use this solver if your system is slightly 
stiff, to avoid numerical damping; 

• ode23tb, implicit Runge-Kutta method solver combining trapezoidal 
rule and a backward differentiation rule of order 2. More efficient 
than odel5s if the solution has many discontinuities; 

• ode23s, Modified Rosenbrock method solver of order 2. This solver is 
also more efficient than odelSs, if the solution has many discontinu¬ 
ities. 

In the developed simulator we used the ode23s solver with Forward dynamics 
analysis. 
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4.4 Connecting to the Virtual Reality tools 


To connect to the VR Sink the signals processed by the simulator developed 


we must treat them (in Figure 4.9 ), we need to apply a rotation from 
world-frame of the simulator to the 3D environment world-frame. 





Figure 4.9: 3D visualization 


The objects in the environment can change the properties of the signals 
through Simulink. Among these properties we mention the most relevant, 
used in the simulator: rotations and translations. The signals for the rota¬ 


tions are introduced (in Figure 4.10 ) as a vector of four elements: indeed, 
they follow an axis-angle representation, three elements for the axis and one 
for the rotation in radians. For the translation we need a vector, it contains 
the position coordinates (opportunely shifted if in the 3D world we have a 
different world frame from the one used in SimMechanics). 

We have identified a substantial difference between the coordinate system 


(in Figure 4.11 ) of SimMechanics with the one of the VR Builder: 

• axis X of SimMechanis, it is the same in the two environments; 

• axis y of SimMechanis, it corresponds in the virtual (-z)-axis] 

• axis z of SimMechanics, it corresponds to the y axis in the environ¬ 
ment. 
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Figure 4.10: Signals for the rotations. 


There was 
and of the 


therefore a necessary correction of the axis expressed in rotations 


components used in translations (in Figure 4.12 


4.4.1 VRML: the Virtual Reality Modeling Language. 

The 3D software used to model the robot and the environment is the one 
provided by Matlab: Logos VR Builder . You can make a 3D scene using 
the instruments given by the software or by writing code. 

The language to use is the VRML (Virtual Reality Modeling Language). 
It is a standard file format for representing 3D interactive vector graphics, 
designed particularly for the World Wide Web. VRML is a text file format 
where, e.g., vertices and edges for a 3D polygon can be specihed along with 
the surface colour, UV mapped textures, shininess, transparency, and so on. 

It has a format similar to XML. Theoretically, the objects can contain 
anything (3D geometry, MIDI data, JPEG images, etc..). VRML defines a 
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Figure 4.11: Axis World in SimMechanics 

set of objects useful for doing 3D graphics. These objects are called Nodes. 

Nodes are arranged in hierarchical structures called scene graphs. A 
node has the following characteristics: 

• What kind of object it is; 

• The parameters that distinguish this node from other nodes of the same 
type; 

• A name to identify this node; 

• Child nodes. 

It offers basic primitives to make up a scene: 

• Sphere; 

• Cone; 

• Cube; 

• Triangle; 

• Box; 

• Cylinder; 
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• ElevationGrid; 


• Extrusion; 

• IdexedFaceSet; 

• IndexedLineSet; 

• Points et; 

• Text; 

The Box, Cone, Cylinder, and Sphere are geometric primitives. The 
Text node displays a string with a specified font style. The Worldinfo node 
holds the world’s title and other information, such as author and copyright. 
The ElevationGrid node creates surfaces and terrains. The Extrusion node 
creates solids by sweeping a 2D cross-section though a 3D spine. The In- 
dexedFaceSet, IndexedLineSet, and PointSet nodes use Coordinate nodes to 
create solid faces, lines, and points, respectively. These raw geometry nodes 
give you more flexibility than the geometric primitives, and can actually 
create more efficient VRML worlds. 

Each node can be inserted in a group. There is a hierarchical structure of 
the scene. Each node of the Kuka Youbot model has to respect the hierarchy 
of the joints of the robot. 

There are also some basics camera and light features. We can insert in 
the scene the following types of light: 

• point light] 

• spot light] 

• directional light. 

We can define geometrical properties of the shape such as the ones for the 
material: 
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ambientintensity; 


• diffuseColor] 

• emissiveColor] 

• shininess ] 

• specular Color] 

• trasparency. 

You can control the diffuse (shading) color, emissive (glow) color, trans¬ 
parency, shininess, and the other optical properties of an object using its 
Appearance node’s Material field. These optical properties interact with 
the scene lighting to determine the image presented to the viewer. 

Only some kinds of materials are well described by the optical properties 
specified in the Material field. Metal and glass are; wood, tile, and painted 
objects are not. You can override optical properties with Color nodes, or 
wrap textures around the 3D shapes. VRML supports three kinds of texture 
mapping fields: ImageTexture (from JPEG, PNG, and GIF files), PixelTex- 
ture (from raw image data), and MovieTexture (from MPEGl files). If you 
use an image with an alpha channel for texture, you can create ’’holes” in 
the texture mapping that allow the underlying material properties to show 
through. 

Ground color, sky color, and background textures are defined by the 
Background node. Multiple backgrounds can be kept in a stack and bound 
dynamically. Atmosphere and an increased sense of depth can be created 
by using a Fog node. You can provide Viewpoint nodes to help the user 
navigate your world. 

VRML files measure distance in meters, angles in radians, time in sec¬ 
onds, and colours as RGB triplets with each value in the range [0.0,1.0]. 
It uses a Gartesian, right-handed three-dimensional coordinate system. By 
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default, the viewer is positioned along the positive Z-axis so as to look along 
the -Z direction with +Y-axis up. The Translation and Rotations have these 
representations : 

• translation: single vector with three elements; 

• rotation: a vector of four element, for the axis-angle representation. 



Figure 4.12: components for the translations 


4.5 Obstacle modelling 


In SimMechanics it is simple to model obstacles, they are represented by 
a body block, where we have to specify the moment of inertia tensor, the 
mass and obviously the position in the space where the obstacle is placed. 
We report here some moment of inertia tensor used to model obstacles. The 
first one is a solid sphere of radius r and mass m: 


^sphere 
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We used also a solid cuboid of width w, height h, depth d, and mass m 
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and a solid cylinder of radius Vc, height he and mass me- 
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In some cases we used the analytic geometry equations of different el¬ 
ements. For example we mention the equation of a sphere with center in 
(xo, Vo, Zo) and radius r. The points on the sphere with radius r can be 
parametrised with following equations: 


X = Xo + rcos{9)sin{4>) 
y = Vo + rsin{9)sin{4>) 
z = Zo + rcos{4>) 


(4.1) 

(4.2) 

(4.3) 


with (0 < 0 < 27r and 0 < (pn). The sensor unit developed takes the 
geometric information by the object model used and then it computes the 
needed distances and the unit vectors. 
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Chapter 5 

Results 


In this chapter we show some case studies selected to explain how the new 
approach works. The first one shows the improvement obtained with the 
new task combination function, emphasizing the chattering phenomena ab¬ 
sence . In the second one two robots move independently in a room with 
some moving obstacles. The third one highlights how the Kuka Youbot end 
effector executes the obstacle avoidance when there is an obstacle placed on 
its path. The fourth case study, the manipulator end effector is commanded 
to stay still and a moving spherical obstacle follows a trajectory that would 
collide with the arm. In the last test, the end effector follows a path to go 
to catch a ball in the space: firstly the scene is composed just of the ball 
afterwards we put in a table and a obstacle, between the ball and the end 
effector. 

5.1 First case study: one robot and one obstacle 

As we said in the Chapter we introduce a task combination function to 
remove the chattering velocity obtained by a crispy supervisor. In the case 
study we want the Kuka Youbot follows a desired path. 

We placed a spherical obstacle at position (2.5m, 2.9m). The object has 
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a diameter equal to 0.30m. To pursuit the obstacle avoidance the robot 
followed the path reported in Figure [5T| For this first case study, we report 



Figure 5.1: The planned path, and the one obtained with the obstacle avoid¬ 
ance. 


the parameters values used in the simulations (the sensors are distributed 
only on the base of the robot): 

• N=8; 

• 7o=l,7ff=2; 

• ts = 0.001s; 


• / = 0.40m; 


Tfc = 0.9m. 


The pseudo-energy trend associated to the eight sensors is showed in Figure 


5.2, when the a function is different from zero, the obstacle avoidance be¬ 


haviour is activated. Figures |5.3| and 5.4 report the x and y components of 
the mobile base velocity obtained with the classical crisp combination law of 
the two tasks and with the new task combination function. It is evident how 
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Figure 5.2: Final history of the pseudo-energy in the first case study. 

the first combination law leads to a chattering velocity, while the proposed 
method generates smoother velocities. 



Figure 5.3: x component of the velocity [m/s]. 


In this case study we compared the artificial potential method (see Chap¬ 
ter with ours, as it is depicted in the Figure 5.5, our method responds 
more quickly then the other one, requiring less time to come back to the orig¬ 
inal path: because the artihcial potential tends to infinity when the robot 
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Figure 5.4: y component of the velocity [m/s]. 

is near the obstacle, it chooses to follow a path as not close to the original 
path as the ours does. 


5.2 Second case: two robots in a room with mov¬ 
ing obstacles 

The second case study takes into account two Kuka Youbot robots moving 
separately on two different paths. For each robot there is a control unit, 
both sharing the same environment. The Figure |5.6| is a collection of this 
case’s snapshots. The time sequence is from left to right and from top to 
bottom. In this case study the parameters values are the same as reported 


in the first one, for both robots. The Figure 5.7 shows the obtained paths, 
which show that significant adjustments are needed to avoid collision with 
humans, hxed obstacles and other robots. In the environment we put not 
only fixed obstacles (desks and bookshelves) but also some moving obstacles 
(human beings, to simulate a possible real case where mobile robots can be 
used for surveillance). 


66 














Figure 5.5: Comparing between our approach and the artificial potential 
method in the first case study. 


5.3 Third case study: Obstacle on the end-effector 
path 


In this case study, an obstacle is situated on a line segment path followed 
by the Kuka Youbot end effector. The sensors are placed also on the ma¬ 
nipulator links, thus the / are smaller then the previous ones, 70 and 7 ^ 
have the same values as the previous case studies. The Figure [5^ shows the 
modified path and the obstacle. In Figure we plotted the modified path 
and the planned one. Both figures clearly show that the end-effector path is 
locally adjusted to avoid the obstacle and to verify that no collision occurs 
with any part of the robot. 


Figure the 5.10 is a snapshots collection of the manipulator movements. 
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Figure 5.6: Snapshots of robots moving in a room with multiple obstacle. 


5.4 Fourth case study: A still manipulator and a 
moving obstacle 

In this case study a moving obstacle tries to hit a robot: the Kuka Youbot 
avoids the obstacle and try to keep the end effector in the desired position. 
Sixteen sensors are mounted on the base and on the manipulator of the 
robot, the /s relative to the manipulator’s sensor points are smaller then 
the ones associated to the sensors on the base. Also for this case the values 


of 7 o and jg are equal to 1. The snapshots reported in Figure 5.11 show 
that the robot moves so as to avoid the obstacle trying to keep the end 
effector at the desired position. Note that in the case of multiple non¬ 
conflicting tasks the NSB does not guarantee that the lower priority task 
is instantaneously achieved with the sub-optimal velocity. Nevertheless, in 
the considered case, the closed loop ensures that the error of secondary task 
converges to zero. This means that the end-effector returns to the desired 
position only after a transient of the motion needed to avoid the obstacle. 
This is proved by the path actually followed by the end effector reported 


in Figure 5.12 which returns to the desired location indicated by the red 
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Figure 5.7: The paths followed by the robots of the second case study. 

star. Furthermore we report the configuration variables q = \ql, q2, ■ ■ ■ , g8] 
obtained by the executed simulation, they are displayed in Figures [5 .13 1 and 

Em 


5.5 Fifth case study: picking a ball on a table 


In this test a robot has to follow the planned path. From the point pi = 
(Om, 0.51m, 0.30m) to the point pf = (—0.03m, 3.2m, 0.02m) passing through 
the point pfi = (—0.03m, 3m, 0.05m) using two line segments. In the point 
pf we put a ball to be picked up. In the scene we put, gradually, two ob¬ 
stacles: a table and a small box. The table is centred in the scene in a way 
to have the ball on it. The small box is put between the point pf, and pfi. 
For this case study we implemented the task combination function written 


in (3.15). Figures 5.15, 5.16, 5.17 are a collection of snapshots for the case 


in study. 


Figure 5.18 shows the CLIK convergence in terms of the end-effector 


position error with the table and the small box. Figure 5.19 reports the 


error obtained in a simulation where there is only the table and the ball. 
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Figure 5.8: The modified path and the obstacle of the third case study. 


In both cases, we plot the end effector path: the planned one and the one 


computed by the obstacle avoidance algorithm (Figures 5.20 and 5.21) . 
In this case study we used the following parameters: 


• N=17, we added a sensor on the top of the end effector, we turn it off 
when the distance between the end effector and the ball is less then 
5cm; 

• rfc=5m, for each sensor; 

• the / are: 0.35m for the sensors on the base, 0.3m for the sensors on 
the first link, 0.1m the ones on the second link and 0.02m the one 
mounted on the end effector; 


• 73 is 2; 

• 7o is 1; 


fo is 0.01s; 


task combination parameter e is 0.08m (see equation (3.15)) 


70 
























Figure 5.9: The modified and planned paths of the third case study. 

The use of a linear task combination function lets the robot move in a more 
fluid way then the sigmoidal one. The chattering phenomena can occur if 
the e parameter is too small. 
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Figure 5.10: Snapshots of the robot motion in the third case study. 



Figure 5.11: Snapshots of the robot motion in the fourth case study. 
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Figure 5.12: The end effector path of the fourth case study. 



Figure 5.13: The first four configuration variables of the fourth case study. 
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Figure 5.14: The second four configuration variables of the fourth case study. 



Figure 5.15: Snapshots of the robot motion in the fifth case study, without 
considering obstacles. 
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Figure 5.16: Snapshots of the robot motion in the fifth case study, with 
object on the table. 



Figure 5.17: Snapshots of the robot motion in the fifth case study, with 
objects on the table and a small box. 
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Figure 5.18: The end effector position error in the fifth case study (with the 
table and the box). 



Figure 5.19: The end effector position error in the fifth case study (with 
only the table) 
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Figure 5.20: The paths in the fifth case study (with only the table). 
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Figure 5.21: The paths in the fifth case study (with table and wall). 
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Chapter 6 

Conclusions and future 
developments 


The new approach satisfy to the requirements listed in Chapter The 
method proposed in this work to address the obstacle avoidance problem for 
a mobile manipulator exploits information provided by distributed proximity 
perception so as to avoid collision of all the parts of the robot with objects as 
well as self-collisions. The algorithm requires very limited a priori knowledge 
about the 3D model of the robot and no information on the environment 
and thus it is suitable for robotic tasks executed in a dynamic, unstructured 
environment. There is no a standard analytics procedure to place the sensors 
on the robots, you can place them randomly. We suggest to cover all the 
part of the robot to protect. 

For its non-conservative approach it can be used on humanoid robots, 
to permit them to interact with the dynamically world in a safer and au¬ 
tonomous way. 

This objective has been achieved by adopting a behaviour based control 
approach with two key innovations. 

First, the definition of the pseudo-energy only on the basis of the dis¬ 
tributed proximity perception and it does not require visual information. 
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This strategy can be physically interpreted as a sort of elastic buffer cover¬ 
ing the parts of the robot around each sensor point when this point comes 
close to an obstacle. 

The second innovation is the introduction of the concept of the task 
combination function, that consists in a more flexible and general way to 
combine multiple tasks in order to obtain a smooth motion of the robot 
avoiding chattering phenomena. 

Another feature of the method is the strict coordination between the 
base and the arm exploiting the redundant degrees of freedom, a relevant 
topic in mobile manipulation. 

The differences with other methods can be summarized as follows: 

• the distributed management of the control points is similar to that of 
the artificial potential but, since we define the priority of the tasks 
there is no need to define the potential that tend to infinity when 
the robot is near the obstacle, it means we need less computation also 
because the information of all the sensors is not always needed, we need 
only the information of the sensors which are activated compressed 
springs ; 

• we guarantee an autonomous handling of task priorities thanks to the 
presence of the NSB approach, and differently from the classical NSB 
we defined a regularized supervisor, the task combination function to 
avoid a chattering robot velocity. 

Obviously we expect to study some more use cases to prove the poten¬ 
tiality of the described approach, especially for humanoid robots. 

As possible future developments, a first extension is to consider the case 
of multiple robots. Owing to the NSB approach, the coordination among 
robots of a team can be achieved by introducing an additional behaviour. 

Another interesting future line of research is the extension of the be- 
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havioural approach based on distributed perception to the physical interac¬ 
tion between robots and the environment, possibly including humans. This 
would require more sophisticated sensing systems able to provide tactile in¬ 
formation but the method to handle such data could be the same proposed 
here. 
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